#include <stdio.h>
#include "LineSegment.h"
#include "GL/glut.h"
#include "include.h"
#include "Object.h"
LineSegment::LineSegment()
{
	left = Vector2D(0.0, 0.0);
	midpoint = Vector2D(0.5,0.0);
	right = Vector2D(1.0, 0.0);
	direction = right;
	normal = Vector2D(0.0, 1.0);
	length = 1.0;
	maxStaticFrictionalForce = DEFAULT_MAX_STATIC_FRICTIONAL_FORCE;
}
LineSegment::LineSegment(Vector2D left, Vector2D right, float staticFrictionalForce)
{
	this->left = left;
	this->right = right;
	
	direction = right - left;//left - right;
	length = direction.Magnitude();
	direction = !direction;
	
	midpoint = (left + right) * 0.5;
	
	normal = Vector2D(-direction.y, direction.x) * -1.0;
	maxStaticFrictionalForce = staticFrictionalForce;
}
LineSegment::LineSegment(Vector2D left, Vector2D right)
{
	this->left = left;
	this->right = right;
	
	direction = right - left;//left - right;
	length = direction.Magnitude();
	direction = !direction;
	
	midpoint = (left + right) * 0.5;
	
	normal = Vector2D(-direction.y, direction.x)* -1.0;
	maxStaticFrictionalForce = DEFAULT_MAX_STATIC_FRICTIONAL_FORCE;
}

bool LineSegment::Collide(Object * object, float * distance, Vector2D * resultantForce, Vector2D * origin)
{
	//we collide if any of the vertices in the object are on the "other" side of the line
	
	if( object->position.dist(midpoint) > length/2.0 + object->boundingRadius)
		return false;
	if(!Collide(object, origin))
		return false;
		
	Vector2D outsidePoint = midpoint + normal;//length;
	Vector2D translationalAxis =  -1.0 * normal;
	bool didCollide = false;
	
	*distance = -1.0;
	
	for(int i = 0; i < object->numVertices; i++)
	{
		Vector2D  vertex = object->vertices[i] + object->position;
		
		Vector2D P3 = right;
		Vector2D P4 = left;
		Vector2D P1 = outsidePoint;
		Vector2D P2 = vertex;
		/*float uaNumerator = (vertex.x - outsidePoint.x) * (left.y - outsidePoint.y) - 
						 	(vertex.y - outsidePoint.y) * (left.x - outsidePoint.x);
		float denominator = (vertex.y - outsidePoint.y) * (right.x - left.x) - 
							(vertex.x - outsidePoint.x) * (right.y - left.y);
		
		float ubNumerator = (right.x - left.x) * (left.y - outsidePoint.y) - 
							(right.y - left.y) * (left.x - outsidePoint.x);
		*/
		
		
		float dx = P2.x - P1.x;
		float dy = P2.y - P1.y;
		float da = P4.x - P3.x;
		float db = P4.y - P3.y;
		float ua = (dx * (P3.y - P1.y) + dy * (P1.x - P3.x)) / (da * dy - db * dx);
    	float ub = (da * (P1.y - P3.y) + db * (P3.x - P1.x)) / (db * dx - da * dy);
		
		if(da * dy - db * dx != 0)//if(denominator != 0.0)
		{

			
			//float ua = uaNumerator/denominator;
			//float ub = ubNumerator/denominator;
			//float ua = (dx * (P3.y - P1.y) + dy * (P1.x - P3.x)) / (da * dy - db * dx);
    		//float ub = (da * (P1.y - P3.y) + db * (P3.x - P1.x)) / (db * dx - da * dy);

			if(ua <= 0 || ua >= 1 || ub <= 0 || ub >= 1)//(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1)////
			{
				
				//this vertex is on the other side of the line segment. 
				//if we go back in the negative normal direction, where will we hit the line segment?
				
				
				didCollide = true;
				//object->isPinned = true;
				Vector2D towardNormal = vertex - normal;
				P1 = left;
				P2 = right;
				P3 = towardNormal;
				P4 = vertex;
				dx = P2.x - P1.x;
				dy = P2.y - P1.y;
				da = P4.x - P3.x;
				db = P4.y - P3.y;
				
				/*float uaNumerator = (vertex.x - towardNormal.x) * (left.y - towardNormal.y) - 
							  (vertex.y - towardNormal.y) * (left.x - towardNormal.x);
				float denominator = (vertex.y - towardNormal.y) * (right.x - left.x) - 
							  (vertex.x - towardNormal.x) * (right.y - left.y);
				float ubNumerator = (right.x - left.x) * (left.y - towardNormal.y) - 
							  (right.y - left.y) * (left.x - towardNormal.x);
				ua = uaNumerator/denominator;
				ub = ubNumerator/denominator;*/
				ua = (dx * (P3.y - P1.y) + dy * (P1.x - P3.x)) / (da * dy - db * dx);
    			ub = (da * (P1.y - P3.y) + db * (P3.x - P1.x)) / (db * dx - da * dy);
    			
				Vector2D collision = Vector2D(P1.x + ub * dx, P1.y + ub * dy);//left.x + ua * (right.x - left.x), left.y + ua * (right.y - left.y));
				float collisionDistance = collision.dist(vertex);
				
				if(collisionDistance > *distance)
				{
					*distance = collisionDistance;
					*origin = collision;
				}
			}
		}
		else if (ua == 0.0 && ub == 0.0)
		{
			printf("Overlapping lines!!!\n");	
		}
		else
			printf("Parallel lines\n");
	}
		
	
	if(didCollide)
	{
		*origin = *origin - object->position;
		Vector2D velocityAtPoint1 = object->VelocityAtPoint(*origin);
		//translationalAxis = !velocityAtPoint1 * -1.0;
		//translationalAxis = !translationalAxis;
		float velocityProjection1 = translationalAxis.dot(velocityAtPoint1);
		
		if(velocityProjection1 > 0.0)
		{
			return false;
		}
		//object->collisionPoint = *origin;
		//object->collisionValid = true;
		
		float velocityChange1 = -1.0 * velocityProjection1;
	
		*resultantForce = (velocityChange1 * object->mass * translationalAxis)/object->lastElapsedTime;
	}
	
	return didCollide;	
	
}
bool LineSegment::Collide(Object * object, Vector2D * origin)
{
	//we collide if any of the edges of the object cross the line
	
	if( object->position.dist(midpoint) > length/2.0 + object->boundingRadius)
		return false;
		
	bool didCollide = false;
	float distance = 0;
	for(int i = 0; i < object->numVertices - 1; i++)
	{
		Vector2D  vertex = object->vertices[i] + object->position;
		
		Vector2D P3 = right;
		Vector2D P4 = left;
		Vector2D P1;
		if(i < object->numVertices)
			P1 = object->vertices[i+1] + object->position;
		else
			P1 = object->vertices[0] + object->position;
		Vector2D P2 = vertex;
		
		float dx = P2.x - P1.x;
		float dy = P2.y - P1.y;
		float da = P4.x - P3.x;
		float db = P4.y - P3.y;
		
		if(da * dy - db * dx != 0)//if(denominator != 0.0)
		{

			
			//float ua = uaNumerator/denominator;
			//float ub = ubNumerator/denominator;
			float ua = (dx * (P3.y - P1.y) + dy * (P1.x - P3.x)) / (da * dy - db * dx);
    		float ub = (da * (P1.y - P3.y) + db * (P3.x - P1.x)) / (db * dx - da * dy);

			//printf("ua = %f, ub = %f\n", ua, ub);
			if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1)
			{
				
				//these segments intersect. Where?
				didCollide = true;
				Vector2D collision = Vector2D(P1.x + ub * dx, P1.y + ub * dy);
				float collisionDistance = collision.dist(vertex);
				
				if(collisionDistance > distance)
				{
					distance = collisionDistance;
					*origin = collision;
				}
			}
		}
	}
		
	
	if(didCollide)
	{
		*origin = *origin - object->position;
	}
	
	return didCollide;	
	
}
bool LineSegment::Collide(LineSegment * segment, Vector2D * origin)
{
	if( segment->midpoint.dist(midpoint) > length/2.0 + segment->length/2.0)
		return false;
		
	Vector2D P3 = right;
	Vector2D P4 = left;
	Vector2D P1 = segment->left;
	Vector2D P2 = segment->right;
	
	float dx = P2.x - P1.x;
	float dy = P2.y - P1.y;
	float da = P4.x - P3.x;
	float db = P4.y - P3.y;
	
	if(da * dy - db * dx != 0)//if(denominator != 0.0)
	{

		float ua = (dx * (P3.y - P1.y) + dy * (P1.x - P3.x)) / (da * dy - db * dx);
		float ub = (da * (P1.y - P3.y) + db * (P3.x - P1.x)) / (db * dx - da * dy);

		//printf("ua = %f, ub = %f\n", ua, ub);
		if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1)
		{
			
			//these segments intersect. Where?
			Vector2D collision = Vector2D(P1.x + ub * dx, P1.y + ub * dy);
			*origin = collision;
			return true;
		}
	}
	else
		printf("DENOMINATOR WAS ZERO\n");
	return false;
}
bool LineSegment::Collide(Object * object, float * distance, float * error, Vector2D * resultantForce, Vector2D * origin)
{
	float dist = object->position.dist(midpoint);
	float maxDistance = (object->boundingRadius + length/2.0);

	if(dist > maxDistance)
		return false;

	Vector2D finalVertex; // this is the vertex of one object that collides with the edge of another
	Vector2D translationalAxis; 
	float minIntervalDistance = maxDistance;
	float minError = 1000000000.0;
	
	//loop through the edges of object1, find a orthogonal ray, and project the 
	//vertices of each object onto it. If the projected lines don't overlap, 
	//we found a dividing edge.
	//If they do overlap, check to see if this is the minimum overlap distance.
	//If it is, keep track of the vertices making up the edge
	Vector2D edge = normal;
	
	float min1,max1;
	float min2,max2;
	Vector2D minVertex1, maxVertex1;
	Vector2D minVertex2, maxVertex2;
	float minError1, maxError1;
	float minError2, maxError2;
	
	object->ProjectPolygon(edge, &min1, &max1, &minVertex1, &maxVertex1, &minError1, &maxError1);
	ProjectLine(edge, &min2, &max2, &minVertex2, &maxVertex2, &minError2, &maxError2);
	//min2 = 0.0;
	//max2 = 0.0;
	
	
	Vector2D point1 = Vector2D(min1, max1);
	Vector2D point2 = Vector2D(min2, max2);

	float intervalDistance = IntervalDistance(point1, point2, minVertex1, maxVertex1, minError1, maxError1, &finalVertex, &minError);
	
	if(intervalDistance > 0)
	{
		return false;
	}
	
	minIntervalDistance = abs(intervalDistance);
	translationalAxis = -1.0 * edge;	

	Vector2D velocityAtPoint1 = object->VelocityAtPoint(finalVertex);
	float velocityProjection1 = translationalAxis.dot(velocityAtPoint1);
	
	//object->collisionPoint = finalVertex;
	//object->collisionValid = true;
	
	float velocityChange1 = -1.2 * velocityProjection1;

	*resultantForce = (velocityChange1 * object->mass * translationalAxis)/object->lastElapsedTime;
	*origin = finalVertex;
	*error = minError;
	*distance = minIntervalDistance;
	return true;	
	
}

void LineSegment::Draw()
{
	glColor3f(0,1,0);
	
	glBegin(GL_LINES);
	glVertex2f(left.x, left.y);
	glVertex2f(right.x, right.y);
	
	glColor3f(1,0,0);
	glVertex2f(midpoint.x, midpoint.y);
	Vector2D midNor = midpoint+normal;
	glVertex2f(midNor.x, midNor.y);
	glEnd();
}
void LineSegment::ProjectLine(Vector2D axis, float * min, float * max, Vector2D * minVertex, Vector2D * maxVertex, float * minError, float * maxError)
{
	
	float dotProduct = axis.dot(left);
	*min = dotProduct;
	*max = dotProduct;
	*minVertex = midpoint;
	*maxVertex = midpoint;
	*minError = length;
	*maxError = length;
} 

	